package LeetDev.a3d;

public class Matrix {
	public static final Matrix identity=new Matrix();

	public float m[]=new float[16];

	public Matrix()
	{
		m[0]=m[5]=m[10]=m[15]=1.0f;
		m[1]=m[2]=m[3]=	m[4]=m[6]=m[7]= m[8]=m[9]=m[11]= m[12]=m[13]=m[14]=0;
	}
	
	public Matrix(final float v)
	{
		m[0]=m[5]=m[10]=m[15]=v;
		m[1]=m[2]=m[3]=	m[4]=m[6]=m[7]= m[8]=m[9]=m[11]= m[12]=m[13]=m[14]=0;
	}
	
	public Matrix(final float w,final float x,final float y,final float z)
	{
		set(w,x,y,z);
	}
	
	public Matrix(final float xa,final float ya,final float za)
	{
		set(xa,ya,za);
	}
	
	public Matrix(final Matrix mt)
	{
		for(int i=16;--i>=0;)
		{
			m[i]=mt.m[i];
		}
	}
	
	public Matrix(final Vect pos, final Vect scale, final Quat angle)
	{
		set(angle);
		scale(scale.x,scale.y,scale.z);
		m[12]=pos.x;m[13]=pos.y;m[14]=pos.z;
	}

	public Matrix(final Quat q)
	{
		set(q.w,q.x,q.y,q.z);
	}
	
	public void set(final Quat q)
	{
		set(q.w,q.x,q.y,q.z);
	}
	
	public void set(final float w,final float x,final float y,final float z)
	{
		float xx = x * x;
		float xy = x * y;
		float xz = x * z;
		float xw = x * w;

		float yy = y * y;
		float yz = y * z;
		float yw = y * w;

		float zz = z * z;
		float zw = z * w;

		m[0] = 1 - 2 * ( yy + zz );
		m[4] =     2 * ( xy - zw );
		m[8] =     2 * ( xz + yw );

		m[1] =     2 * ( xy + zw );
		m[5] = 1 - 2 * ( xx + zz );
		m[9] =     2 * ( yz - xw );

		m[2] =     2 * ( xz - yw );
		m[6] =     2 * ( yz + xw );
		m[10] = 1 - 2 * ( xx + yy );

		m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
		m[15]=1;
	}

	public void set(final float xa,final float ya,final float za)
	{
		float cr, cp, cy, sr, sp, sy, cpcy, spsy;
		  
		cr=(float)Math.cos(xa/2.0f);
		cp=(float)Math.cos(ya/2.0f);
		cy=(float)Math.cos(za/2.0f);
	  
		sr=(float)Math.sin(xa/2.0f);
		sp=(float)Math.sin(ya/2.0f);
		sy=(float)Math.sin(za/2.0f);
	  
		cpcy=cp*cy;
		spsy=sp*sy;
	  
		set(cr*cpcy+sr*spsy, sr*cpcy-cr*spsy, cr*sp*cy+sr*cp*sy, cr*cp*sy-sr*sp*cy);
	}
	
	public void transform(final Vect v, Vect result)
	{
		result.x=m[0]*v.x+m[4]*v.y+m[8]*v.z+m[12];
		result.y=m[1]*v.x+m[5]*v.y+m[9]*v.z+m[13];
		result.z=m[2]*v.x+m[6]*v.y+m[10]*v.z+m[14];
	}

	// This is not a correct 4*4 inverse.
	// It inverses the top left 3*3 matrix and then inverses the "position" vector[0][3]-[2][3]
	// However if we only work with 3*3 transforms and positions, then this function will fullfill m*m'=1
	public void inverse(Matrix result)
	{
		float x;
		result.m[0]=m[6]*m[9]-m[5]*m[10];
		result.m[4]=-(m[6]*m[8]-m[4]*m[10]);
		result.m[8]=m[5]*m[8]-m[4]*m[9];
		x=1/(result.m[0]*m[0]+result.m[4]*m[1]+result.m[8]*m[2]);
		result.m[0]*=x;
		result.m[4]*=x;
		result.m[8]*=x;
		result.m[1]=-(m[2]*m[9]-m[1]*m[10])*x;
		result.m[5]=(m[2]*m[8]-m[0]*m[10])*x;
		result.m[9]=-(m[1]*m[8]-m[0]*m[9])*x;
		result.m[2]=(m[2]*m[5]-m[1]*m[6])*x;
		result.m[6]=-(m[2]*m[4]-m[0]*m[6])*x;
		result.m[10]=(m[1]*m[4]-m[0]*m[5])*x;

		result.m[12]=-(result.m[0]*m[12]+result.m[4]*m[13]+result.m[8]*m[14]);
		result.m[13]=-(result.m[1]*m[12]+result.m[5]*m[13]+result.m[9]*m[14]);
		result.m[14]=-(result.m[2]*m[12]+result.m[6]*m[13]+result.m[10]*m[14]);
		result.m[3]=result.m[7]=result.m[11]=0;
		result.m[15]=1;
	}
	
	public void scale(float x, float y, float z)
	{
		m[0]*=x;m[4]*=x;m[8]*=x;m[12]*=x;
		m[1]*=y;m[5]*=y;m[9]*=y;m[13]*=y;
		m[2]*=z;m[6]*=z;m[10]*=z;m[14]*=z;
	}
	
	public void mul(final Matrix ma, Matrix result)
	{
		result.m[0]=m[0]*ma.m[0]+m[1]*ma.m[4]+m[2]*ma.m[8]+m[3]*ma.m[12];
		result.m[1]=m[0]*ma.m[1]+m[1]*ma.m[5]+m[2]*ma.m[9]+m[3]*ma.m[13];
		result.m[2]=m[0]*ma.m[2]+m[1]*ma.m[6]+m[2]*ma.m[10]+m[3]*ma.m[14];
		result.m[3]=m[0]*ma.m[3]+m[1]*ma.m[7]+m[2]*ma.m[11]+m[3]*ma.m[15];
		result.m[4]=m[4]*ma.m[0]+m[5]*ma.m[4]+m[6]*ma.m[8]+m[7]*ma.m[12];
		result.m[5]=m[4]*ma.m[1]+m[5]*ma.m[5]+m[6]*ma.m[9]+m[7]*ma.m[13];
		result.m[6]=m[4]*ma.m[2]+m[5]*ma.m[6]+m[6]*ma.m[10]+m[7]*ma.m[14];
		result.m[7]=m[4]*ma.m[3]+m[5]*ma.m[7]+m[6]*ma.m[11]+m[7]*ma.m[15];
		result.m[8]=m[8]*ma.m[0]+m[9]*ma.m[4]+m[10]*ma.m[8]+m[11]*ma.m[12];
		result.m[9]=m[8]*ma.m[1]+m[9]*ma.m[5]+m[10]*ma.m[9]+m[11]*ma.m[13];
		result.m[10]=m[8]*ma.m[2]+m[9]*ma.m[6]+m[10]*ma.m[10]+m[11]*ma.m[14];
		result.m[11]=m[8]*ma.m[3]+m[9]*ma.m[7]+m[10]*ma.m[11]+m[11]*ma.m[15];
		result.m[12]=m[12]*ma.m[0]+m[13]*ma.m[4]+m[14]*ma.m[8]+m[15]*ma.m[12];
		result.m[13]=m[12]*ma.m[1]+m[13]*ma.m[5]+m[14]*ma.m[9]+m[15]*ma.m[13];
		result.m[14]=m[12]*ma.m[2]+m[13]*ma.m[6]+m[14]*ma.m[10]+m[15]*ma.m[14];
		result.m[15]=m[12]*ma.m[3]+m[13]*ma.m[7]+m[14]*ma.m[11]+m[15]*ma.m[15];
	}

	public void special_mul(final Matrix ma, Matrix result)
	{
		result.m[0]=m[0]*ma.m[0]+m[1]*ma.m[4]+m[2]*ma.m[8];
		result.m[1]=m[0]*ma.m[1]+m[1]*ma.m[5]+m[2]*ma.m[9];
		result.m[2]=m[0]*ma.m[2]+m[1]*ma.m[6]+m[2]*ma.m[10];
		result.m[3]=0;
		result.m[4]=m[4]*ma.m[0]+m[5]*ma.m[4]+m[6]*ma.m[8];
		result.m[5]=m[4]*ma.m[1]+m[5]*ma.m[5]+m[6]*ma.m[9];
		result.m[6]=m[4]*ma.m[2]+m[5]*ma.m[6]+m[6]*ma.m[10];
		result.m[7]=0;
		result.m[8]=m[8]*ma.m[0]+m[9]*ma.m[4]+m[10]*ma.m[8];
		result.m[9]=m[8]*ma.m[1]+m[9]*ma.m[5]+m[10]*ma.m[9];
		result.m[10]=m[8]*ma.m[2]+m[9]*ma.m[6]+m[10]*ma.m[10];
		result.m[11]=0;
		result.m[12]=m[12]*ma.m[0]+m[13]*ma.m[4]+m[14]*ma.m[8]+ma.m[12];
		result.m[13]=m[12]*ma.m[1]+m[13]*ma.m[5]+m[14]*ma.m[9]+ma.m[13];
		result.m[14]=m[12]*ma.m[2]+m[13]*ma.m[6]+m[14]*ma.m[10]+ma.m[14];
		result.m[15]=1;
	}
}
